//----------------------------------------------------------------------------------------------
//	Filename:	InputHandler.h
//	Author:		Sandro Spina
//	Date:		7/01/2014
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#pragma once
#include <windows.h>
//----------------------------------------------------------------------------------------------
//	Include basic headers for OpenMP and io, string and file streams
//----------------------------------------------------------------------------------------------
#include <omp.h>
#include <iostream>
#include <sstream>
#include <fstream>

//----------------------------------------------------------------------------------------------
//	Include boost header files for managing program options and file paths
//----------------------------------------------------------------------------------------------
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/thread.hpp>

//----------------------------------------------------------------------------------------------
#include "Logger.h"
#include "Environment.h"
#include "PointCloudSolver.h"

namespace Illumina
{
	namespace Core
	{		
		class InputHandler
		{
		public:
		
			struct KeyboardStates
			{  
				enum key_state { clicked, pressed, released, up, invalid };
				bool m_keyboard_state[256];
 
				KeyboardStates::KeyboardStates()
				{
					for (int i = 0; i<256; i++) m_keyboard_state[i] = false;
				}

				key_state GetKeyState(int p_key)
				{		
					if (p_key < 256)
					{
						bool pressed = (GetAsyncKeyState(p_key) & 0x8000);

						//std::cout << "Key = [" << p_key << "], State = [" << m_keyboard_state[p_key] << "], New = [" << pressed << "]" << std::endl;

						if (m_keyboard_state[p_key])
						{
							m_keyboard_state[p_key] = pressed;

							return (pressed) ? KeyboardStates::pressed
								: KeyboardStates::released;
						}
						else
						{
							m_keyboard_state[p_key] = pressed;

							return (pressed) ? KeyboardStates::clicked
								: KeyboardStates::up;
						}
					}

					return KeyboardStates::invalid;		
				}
 
			};
		
			KeyboardStates key;
		
			boolean accuReset;
			ICamera *pCamera;
			Illumina::Core::Environment *pEnvironment;
			Illumina::Core::EngineKernel *pKernel;
			PointCloudSolver *pcs;
			float fCameraSpeed;
			bool restore;

			RECT vlcRect;	

			AccumulationBuffer *pBuffer;

			float theta_y;
			float theta_x;
			float theta_z;

			float window_width;
			float window_height;

			boolean processing_visibility_request;
			boolean processing_visibility_segmentid;
			boolean processing_visibility_request_delay;
			std::string visibility_segment_number;
	
			boolean processing_visibility_obbsegmentid;
			boolean processing_visibility_obbrequest_delay;

			boolean processing_graph_pruning_request;
			boolean processing_graph_pruning_request_delay;
			std::string graph_pruning_state_number;

			boolean processing_search_request;
			boolean processing_search_request_delay;
			std::string search_shape_id;

			boolean processing_pointcloud_switch_request;
			boolean processing_pointcloud_switch_request_view;
			std::string new_pointcloud_id;
			std::string new_pointcloud_view;
			std::string old_pointcloud_id;
			std::string old_pointcloud_view;

			float grid_rot;
			float grid_scale;
			bool all_points_in_grid;

			float u_min_t;
			float u_max_t;
			float v_min_t;
			float v_max_t;
			float w_min_t;
			float w_max_t;

			int currentlyViewedSegment;
			int currentlyViewedModelVG;

			GeometricPrimitive * gp;
			bool scene_change;
			Ray ray;

			std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * camera_parameters;					

			void SaveCameraParameters(Environment *p_environment, std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * p_cameraParameters)
			{
				std::string camera_parameters_location = p_environment->GetCameraParametersStore();	
				int saved_count = 0;

				//Read file 
				FILE *fp;
				bool quit = false;
				if (!(fp = fopen(camera_parameters_location.c_str(), "w")))
				{
					std::cout << "[CameraParameters] - Unable to open camera parameters file " << camera_parameters_location << std::endl;
					quit = true;
				}
				else
				{		
					std::cout << "[CameraParameters] - Storing camera parameters file " << camera_parameters_location << std::endl;
					std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>>::iterator cpIterator;
					std::string scene_name;
					Vector3 cam_pos, cam_target, cam_up;
					float rot_x, rot_y, rot_z;
					for (cpIterator = p_cameraParameters->begin(); cpIterator != p_cameraParameters->end(); cpIterator++)
					{
						scene_name = cpIterator->first;
						cam_pos		= cpIterator->second.first[0];
						cam_target	= cpIterator->second.first[1];
						cam_up		= cpIterator->second.first[2];

						rot_x = cpIterator->second.second[0];
						rot_y = cpIterator->second.second[1];
						rot_z = cpIterator->second.second[2];

						fputs (scene_name.c_str(),fp);
						fprintf(fp, " %f %f %f %f %f %f %f %f %f %f %f %f\n", cam_pos.X, cam_pos.Y, cam_pos.Z, cam_target.X, cam_target.Y, cam_target.Z, cam_up.X, cam_up.Y, cam_up.Z, rot_x, rot_y, rot_z);
						saved_count++;
					}
					fclose(fp);
				}
				std::cout << "[CameraParametersLoader] Stored " << saved_count << " camera parameters" << std::endl;
			}

			std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * LoadCameraParameters(Environment *p_environment)
			{	
				std::string camera_parameters_location = p_environment->GetCameraParametersStore();
				std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * camera_parameters = new std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>>();

				typedef boost::tokenizer<boost::char_separator<char>> tokenizer;	
				boost::char_separator<char> sep(" \n");
				tokenizer::iterator beg;		

				int loaded_count = 0;

				//Read file 
				FILE *fp;
				bool quit = false;
				if (!(fp = fopen(camera_parameters_location.c_str(), "r")))
				{
					std::cout << "[Training Phase] - Unable to open camera parameters file " << camera_parameters_location << std::endl;
					quit = true;
				}
				else
				{
					std::cout << "[Training Phase] - Reading from camera parameters file " << camera_parameters_location << std::endl;
					char buffer[512];
					int line_count = 0;
					std::string model_name, line_data;
					float cam_position_x, cam_position_y, cam_position_z;
					float cam_target_x, cam_target_y, cam_target_z;			
					float cam_up_x, cam_up_y, cam_up_z;
					float theta_x, theta_y, theta_z;

					while (fgets(buffer, 512, fp))
					{
						line_count++;
						char *bufferp = buffer;
						while (isspace(*bufferp)) bufferp++;
				
						if (*bufferp == '#') continue;   //skip comments
						if (*bufferp == '\0') continue;	 //and blank lines				
				
						line_data = buffer;
						tokenizer tok(line_data, sep);
						beg = tok.begin();
						model_name = *beg; ++beg;

						cam_position_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
						cam_position_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
						cam_position_z = boost::lexical_cast<float, std::string>(*beg); ++beg;

						cam_target_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
						cam_target_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
						cam_target_z = boost::lexical_cast<float, std::string>(*beg); ++beg;

						cam_up_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
						cam_up_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
						cam_up_z = boost::lexical_cast<float, std::string>(*beg); ++beg;

						theta_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
						theta_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
						theta_z = boost::lexical_cast<float, std::string>(*beg);
				
						(*camera_parameters)[model_name] = std::pair<std::vector<Vector3>, std::vector<float>>();
						(*camera_parameters)[model_name].first.push_back(Vector3(cam_position_x, cam_position_y, cam_position_z));
						(*camera_parameters)[model_name].first.push_back(Vector3(cam_target_x, cam_target_y, cam_target_z));
						(*camera_parameters)[model_name].first.push_back(Vector3(cam_up_x, cam_up_y, cam_up_z));
						(*camera_parameters)[model_name].second.push_back(theta_x);
						(*camera_parameters)[model_name].second.push_back(theta_y);
						(*camera_parameters)[model_name].second.push_back(theta_z);

						loaded_count++;
					}
					fclose(fp);
				}
				std::cout << "[CameraParametersLoader] Loaded " << loaded_count << " camera parameters" << std::endl;		
				return camera_parameters;
			}

			InputHandler(Environment *_pEnvironment, EngineKernel *_pKernel, PointCloudSolver *_pcs, float p_fCameraSpeed)
			{
				pKernel = _pKernel;
				pEnvironment = _pEnvironment;
				pCamera = pEnvironment->GetCamera();
				pcs = _pcs;
				fCameraSpeed = p_fCameraSpeed;
				std::cout << "Setting Camera Speed to " << fCameraSpeed << std::endl;
				RECT vlcRect;
				restore = false;

				AccumulationBuffer *pBuffer = (AccumulationBuffer*) pKernel->GetPostProcessManager()->RequestInstance("AccumulationBuffer");				
				theta_y = 0.f;
				theta_x = 0.f;
				theta_z = 0.f;

				window_width = 0;
				window_height = 0;

				processing_visibility_request = false;
				processing_visibility_segmentid = false;
				processing_visibility_request_delay = false;	
	
				processing_visibility_obbsegmentid = false;
				processing_visibility_obbrequest_delay = false;

				processing_graph_pruning_request = false;
				processing_graph_pruning_request_delay = false;	

				processing_search_request = false;
				processing_search_request_delay = false;	

				processing_pointcloud_switch_request = false;
				processing_pointcloud_switch_request_view = false;
	
				grid_rot = 0;
				grid_scale = 1;
				all_points_in_grid = false;

				u_min_t = 0;
				u_max_t = 0;
				v_min_t = 0;
				v_max_t = 0;
				w_min_t = 0;
				w_max_t = 0;

				currentlyViewedSegment = 0;
				currentlyViewedModelVG = 0;

				gp = (GeometricPrimitive*) pEnvironment->GetSpace()->PrimitiveList[0];
				scene_change = false;	

				camera_parameters = LoadCameraParameters(pEnvironment);
				//------------------------
				//Position camera to pre-saved position if one is availalbe
				std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());	
				if (camera_parameters->find(trunk) != camera_parameters->end())
				{
					std::cout << "[Setting Camera Parameters] - Trunk " << trunk << " found. Resetting Camera Position" << std::endl;
					std::pair<std::vector<Vector3>, std::vector<float>> cam_parameters = camera_parameters->find(trunk)->second;
					pCamera->MoveTo(cam_parameters.first[0]);
					pCamera->Look(cam_parameters.first[1], cam_parameters.first[2]);
					theta_x = cam_parameters.second[0];
					theta_y = cam_parameters.second[1];
					theta_z = cam_parameters.second[2];
					gp->WorldTransform.SetRotation(Matrix3x3::CreateRotation(Vector3::UnitXPos, theta_x) *
													Matrix3x3::CreateRotation(Vector3::UnitYPos, theta_y) *
													Matrix3x3::CreateRotation(Vector3::UnitZPos, theta_z));		
					accuReset=true;
				}
				//------------------------
			}			

			bool HandleInput()
			{
				accuReset = false;
				if (!processing_visibility_request && !processing_pointcloud_switch_request)
				{						
					scene_change = false;
					//Camera Movements
					if (key.GetKeyState('W') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetW() * fCameraSpeed); scene_change = true; }
					if (key.GetKeyState('S') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetW() * -fCameraSpeed); scene_change = true; }
					if (key.GetKeyState('A') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetU() * fCameraSpeed); scene_change = true; }
					if (key.GetKeyState('D') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetU() * -fCameraSpeed); scene_change = true; }					 
					if (key.GetKeyState('Q') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetV() * fCameraSpeed); scene_change = true; }
					if (key.GetKeyState('E') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetV() * -fCameraSpeed); scene_change = true; }

					//Splat size adjustment
					if (key.GetKeyState(VK_ADD) == KeyboardStates::clicked) { ((KDTreePointCloud*)(gp->GetShape()))->SetRadius(((KDTreePointCloud*)(gp->GetShape()))->GetRadius() + 0.01); scene_change = true; }
					if (key.GetKeyState(VK_SUBTRACT) == KeyboardStates::clicked) { ((KDTreePointCloud*)(gp->GetShape()))->SetRadius(((KDTreePointCloud*)(gp->GetShape()))->GetRadius() - 0.01); scene_change = true; }

					//PointCloud Movements
					if (key.GetKeyState(VK_NUMPAD8) == KeyboardStates::clicked) { theta_x-=0.1f; scene_change = true; }					
					if (key.GetKeyState(VK_NUMPAD2) == KeyboardStates::clicked) { theta_x+=0.1f; scene_change = true; }						
					if (key.GetKeyState(VK_NUMPAD4) == KeyboardStates::clicked) { theta_y-=0.1f; scene_change = true; }						
					if (key.GetKeyState(VK_NUMPAD6) == KeyboardStates::clicked) { theta_y+=0.1f; scene_change = true; }						
					if (key.GetKeyState(VK_NUMPAD7) == KeyboardStates::clicked) { theta_z-=0.1f; scene_change = true; }					
					if (key.GetKeyState(VK_NUMPAD9) == KeyboardStates::clicked) { theta_z+=0.1f; scene_change = true; }

					//Print|Update Camera Parameters
					if (key.GetKeyState(VK_F9) == KeyboardStates::clicked) { std::cout << "[Current Camera Parameters] " << pCamera->ToString() << std::endl; }
			
					if (key.GetKeyState(VK_F10) == KeyboardStates::clicked) 
					{ 
						//std::string trunk = ((KDTreePointCloud*)(gp->GetShape()))->GetTrunk();				
						//if (camera_parameters->find(trunk) != camera_parameters->end())
						std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());
						if (camera_parameters->find(trunk) != camera_parameters->end())
						{
							std::cout << "[Setting Camera Parameters] - Trunk " << trunk << " found. Resetting Camera Position" << std::endl;
							std::pair<std::vector<Vector3>, std::vector<float>> cam_parameters = camera_parameters->find(trunk)->second;
							pCamera->MoveTo(cam_parameters.first[0]);
							pCamera->Look(cam_parameters.first[1], cam_parameters.first[2]);
							theta_x = cam_parameters.second[0];
							theta_y = cam_parameters.second[1];
							theta_z = cam_parameters.second[2];
							scene_change = true;
						}
						else
						{
							std::cout << "[Setting Camera Parameters] - Trunk " << trunk << " not found in previously stored parameters" << std::endl; 
						}
					}			

					if (key.GetKeyState(VK_F11) == KeyboardStates::clicked)
					{
						std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());				
						std::cout << "[Camera Parameters] Setting Parameters for " << trunk << std::endl;
						(*camera_parameters)[trunk] = std::pair<std::vector<Vector3>, std::vector<float>>();
						(*camera_parameters)[trunk].first.clear();
						(*camera_parameters)[trunk].second.clear();
						(*camera_parameters)[trunk].first.push_back(pCamera->GetObserver());
						(*camera_parameters)[trunk].first.push_back(pCamera->GetFrame().GetW());
						(*camera_parameters)[trunk].first.push_back(pCamera->GetFrame().GetV());				
						(*camera_parameters)[trunk].second.push_back(theta_x);
						(*camera_parameters)[trunk].second.push_back(theta_y);
						(*camera_parameters)[trunk].second.push_back(theta_z);
						scene_change = true;
					}

					if (key.GetKeyState(VK_F12) == KeyboardStates::clicked)
					{
						std::cout << "[Camera Parameters] Persisting Camera Parameters #count = " << camera_parameters->size() << std::endl;
						SaveCameraParameters(pEnvironment, camera_parameters);
					}

					//-----
					if (scene_change) 
					{
						accuReset = true;
						gp->WorldTransform.SetRotation(Matrix3x3::CreateRotation(Vector3::UnitXPos, theta_x) *
														Matrix3x3::CreateRotation(Vector3::UnitYPos, theta_y) *
														Matrix3x3::CreateRotation(Vector3::UnitZPos, theta_z));
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->UpdateWorldTransforms(gp->WorldTransform);
						scene_change = false;
					}
				}		

				//**********************************************************
				//Visibility Settings Processing
				if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('V') == KeyboardStates::clicked)
				{
					processing_visibility_request = true;
					std::cout << "[Main] Entering Visibility Request Mode ... " << std::endl;
					processing_visibility_request_delay = true;
				}

				//Markov Search Settings Processing Flagged
				if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('M') == KeyboardStates::clicked)
				{
					processing_search_request = true;
					std::cout << "[Main] Entering Search Request Mode ... " << std::endl;
					processing_search_request_delay = true;
				}

				//GridView Processing Flagged
				if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('G') == KeyboardStates::clicked)
				{
					//processing_grid_request = true;
					std::cout << "[Main] Viewing VoxelGrid of Currently Set Segment: " << visibility_segment_number << std::endl;
			
					GeometricPrimitive * gp = new GeometricPrimitive();			
					PointCloudSegmentation * model_pcs = pcs->m_trainedModelsDB->GetByName("chairs", new_pointcloud_id);
					if (model_pcs == NULL) std::cout << "MODEL IS NULL ... " << new_pointcloud_id << std::endl;	
					else { 
						std::cout << "MODEL LOADED ... "; model_pcs->ToString(); 
						FeatureDescStructureGraph * fdg = model_pcs->GetFDG(boost::lexical_cast<int, std::string>(visibility_segment_number));
						std::vector<std::pair<std::pair<bool, float>, Vector3>> * grid_spheres = fdg->m_grid_u_inv_v_w_inv->GetAllCells();
						std::cout << "Number of spheres = " << grid_spheres->size() << std::endl;
						KDTreeSphereMesh * sphereMesh = new KDTreeSphereMesh(new_pointcloud_id+"_grid");
						int g = 0; 
						for (auto s : *grid_spheres) 
						{ 
							if (s.first.first) g = 0; else g = 1; 
							sphereMesh->AddSphere(s.second, s.first.second, g); 
						}
						sphereMesh->ComputeBoundingVolume();
						std::cout << "SphereMesh Bounding Volume = " << sphereMesh->GetBoundingVolume()->ToString() << std::endl;
						sphereMesh->Compile();
						gp->SetShape(sphereMesh);
						gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;			
						gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid(new_pointcloud_id+":"+visibility_segment_number, gp);
						scene_change=true;
						accuReset = true;
					}
				}
		
				struct vg_stats {
					FeatureDescStructureGraph::VoxelGrid * vgrid;
					FeatureDescStructureGraph::VoxelGrid * tgrid;
					Vector3 best_min_uvw_t;
					Vector3 best_max_uvw_t;
					float best_rot;
					int min_diff;
					int max_pos;
					int max_neg;				
					int total_vg_built;			
					float max_coverage;
					float coverage;
					int points_out;

					vg_stats(FeatureDescStructureGraph::VoxelGrid * _tgrid)
						: best_rot(0)
						, vgrid(new FeatureDescStructureGraph::VoxelGrid(1,1,1))
						, tgrid(_tgrid)
						, best_min_uvw_t(0)
						, best_max_uvw_t(0)
						, min_diff(INT_MAX)
						, max_pos(0)
						, max_neg(0)
						, total_vg_built(0)
						, max_coverage(0)
						, coverage(0)
						, points_out(0)
					{}									
				};

				//GridView Processing On Scene Segment
				if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('K') == KeyboardStates::clicked)
				{
					//processing_grid_request = true;
					//std::cout << "[Main] Viewing VoxelGrid of Currently Set Segment: " << visibility_segment_number << std::endl;
					((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->ClearAllGrids();

					GeometricPrimitive * gp = new GeometricPrimitive();			
					PointCloudSegmentation * model_pcs = pcs->m_trainedModelsDB->GetByName("chairs", "10000");
					if (model_pcs == NULL) std::cout << "MODEL IS NULL ... " << new_pointcloud_id << std::endl;
					else {
						//std::cout << "MODEL LOADED ... " << model_pcs->ToString() << std::endl;
						FeatureDescStructureGraph * fdg = model_pcs->GetFDG(12);
						int rotationCount = 24;
						float rot_diff = Maths::PiTwo / rotationCount;
						double start = Platform::GetTime();
						float scale_factor = 0.15f;
						std::pair<float, std::pair<float, float>> voxel_grid_measure;

						Vector3 best_obb_scale;
						Vector3 best_min_uvw_t;
						Vector3 best_max_uvw_t;
						float best_rot;
						int min_diff = 1000000;
						int max_pos = 0;
						int max_neg = 0;
						int total_vg_built = 0; 
						float t_step = 0.6f;
						float max_coverage = 0;
						float coverage = 0;
						int best_grid = -1;					
				
						std::vector<vg_stats> model_grids;
						model_grids.push_back(vg_stats(fdg->m_grid_u_v_w));
						model_grids.push_back(vg_stats(fdg->m_grid_u_v_inv_w));
						model_grids.push_back(vg_stats(fdg->m_grid_u_v_w_inv));
						model_grids.push_back(vg_stats(fdg->m_grid_u_v_inv_w_inv));
						model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_w));
						model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_inv_w));
						model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_w_inv));
						model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_inv_w_inv));

						int average_grid_size = (fdg->m_grid_u_v_w->m_voxels->size() + 
										fdg->m_grid_u_v_inv_w->m_voxels->size() +
										fdg->m_grid_u_v_w_inv->m_voxels->size() +
										fdg->m_grid_u_v_inv_w_inv->m_voxels->size() +
										fdg->m_grid_u_inv_v_w->m_voxels->size() +
										fdg->m_grid_u_inv_v_inv_w->m_voxels->size() +
										fdg->m_grid_u_v_w_inv->m_voxels->size() +
										fdg->m_grid_u_inv_v_inv_w_inv->m_voxels->size()) / 8;
				
						FeatureDescStructureGraph::VoxelGrid * crt_model_grid;
						FeatureDescStructureGraph::VoxelGrid * voxel_grid = new FeatureDescStructureGraph::VoxelGrid(1, 1, 1);
				
						//std::vector<int> segs; segs.push_back(374); segs.push_back(375);
						std::vector<int> segs; segs.push_back(372); segs.push_back(373); //segs.push_back(374); //segs.push_back(129);
						//std::vector<int> segs; segs.push_back(369); segs.push_back(370); segs.push_back(371);
						//std::vector<int> segs; segs.push_back(378); segs.push_back(379); segs.push_back(380);
				
						int total_points_in_segs = 0;
						for (auto s : segs)
						{
							total_points_in_segs += pcs->m_sceneToSolve->m_structureGraph->m_nodes->at(s)->segment->points->size();
						}

						#pragma omp parallel for
						for (int vg_idx = 0; vg_idx<model_grids.size(); vg_idx++)
						{
							vg_stats * crt_stats = &(model_grids.at(vg_idx));
							FeatureDescStructureGraph::VoxelGrid * crt_model_grid = crt_stats->tgrid;
							bool all_points_in_grid = false;
							Vector3 min_t(0); Vector3 max_t(0);
							for (int i = 0; i<rotationCount; i++)
							{
								all_points_in_grid = false;
								float inclusion_bias = 0.f;
								min_t.X = 0; min_t.Y = 0; min_t.Z = 0;
								max_t.X = 0; max_t.Y = 0; max_t.Z = 0;
								while (!all_points_in_grid)
								{							
									pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(segs, crt_model_grid, 1, rot_diff*i, min_t, max_t, crt_stats->vgrid);
									crt_stats->total_vg_built++;						
									//crt_stats->coverage = crt_model_grid->ComputeOverlapPosWithGridUVW(crt_stats->vgrid).second / (float)average_grid_size;
									int points_out = crt_stats->vgrid->above_u_max + crt_stats->vgrid->below_u_min +
													crt_stats->vgrid->below_v_min + crt_stats->vgrid->above_v_max +
													crt_stats->vgrid->below_w_min + crt_stats->vgrid->above_w_max;
									if (segs.size() == 1) inclusion_bias = 0.2f; 
									else 
									{
										points_out == 0 ? inclusion_bias = 0.f : inclusion_bias = ((float)points_out / (float)total_points_in_segs) / 5.0f;
									}

									if (crt_stats->coverage - (inclusion_bias) >= crt_stats->max_coverage)
									{								
										crt_stats->max_coverage = crt_stats->coverage - (inclusion_bias);
										crt_stats->points_out = points_out;
										//std::cout << "Inclusion Bias Set to " << inclusion_bias << " Max_Coverage to " << crt_stats->max_coverage << std::endl;
										//voxel_grid_measure = crt_model_grid->ComputeOverlapDiffWithGridUVW(voxel_grid);
										//max_pos = voxel_grid_measure.second.first;
										//max_neg = voxel_grid_measure.second.second;
										//min_diff = voxel_grid_measure.first;	
										crt_stats->best_rot = rot_diff*i;
										crt_stats->best_min_uvw_t.X = min_t.X; crt_stats->best_min_uvw_t.Y = min_t.Y; crt_stats->best_min_uvw_t.Z = min_t.Z;
										crt_stats->best_max_uvw_t.X = max_t.X; crt_stats->best_max_uvw_t.Y = max_t.Y; crt_stats->best_max_uvw_t.Z = max_t.Z; 																							
									}	

									all_points_in_grid=true;
									if (crt_stats->vgrid->above_u_max > 0 | crt_stats->vgrid->below_u_min > 0) { min_t.X += t_step; max_t.X += t_step; all_points_in_grid = false; } // std::cout << " U++ Above::" << voxel_grid->above_u_max << " Below::" << voxel_grid->below_u_min << std::endl; }
									if (crt_stats->vgrid->below_v_min > 0) { min_t.Y += t_step; all_points_in_grid=false; } // std::cout << " BelowV::" << voxel_grid->below_v_min << std::endl; }
									if (crt_stats->vgrid->above_v_max > 0) { max_t.Y += t_step; all_points_in_grid=false; } // std::cout << " AboveV::" << voxel_grid->above_v_max << std::endl; }
									if (crt_stats->vgrid->below_w_min > 0) { min_t.Z += t_step; all_points_in_grid=false; } // std::cout << " BelowW::" << voxel_grid->below_w_min << std::endl; }
									if (crt_stats->vgrid->above_w_max > 0) { max_t.Z += t_step; all_points_in_grid=false; } // std::cout << " AboveW::" << voxel_grid->above_w_max << std::endl; }
								}											
								min_t.X = 0; min_t.Y = 0; min_t.Z = 0;
								max_t.X = 0; max_t.Y = 0; max_t.Z = 0;
							}
						}				
				
						double end = Platform::GetTime();
				
						std::cout << "Time Elapsed::" << Platform::ToSeconds(end-start) << " ... AND THE RESULTS ARE ****** " << std::endl;

						for (int i=0; i<model_grids.size(); i++)
						{
							std::cout << "Grid:" << i << " Max Coverage::" << model_grids.at(i).max_coverage << " Points Out::" << model_grids.at(i).points_out << " Model VG Size::" <<  model_grids.at(i).tgrid->m_voxels->size() << std::endl;				
							if (model_grids.at(i).max_coverage > max_coverage) { max_coverage = model_grids.at(i).max_coverage; best_grid = i; }
							total_vg_built += model_grids.at(i).total_vg_built;
						}
				
						//best_grid = 1;

						crt_model_grid = model_grids.at(best_grid).tgrid;			
						voxel_grid = pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(segs, crt_model_grid, 1, model_grids.at(best_grid).best_rot, model_grids.at(best_grid).best_min_uvw_t, model_grids.at(best_grid).best_max_uvw_t);
						voxel_grid_measure = crt_model_grid->ComputeOverlapDiffWithGridUVW(voxel_grid);
						model_grids.at(best_grid).max_pos	= voxel_grid_measure.second.first;
						model_grids.at(best_grid).max_neg	= voxel_grid_measure.second.second;
						model_grids.at(best_grid).min_diff	= voxel_grid_measure.first;
						std::cout << best_grid << ") Total VGs Computed::" << total_vg_built << " MaxCoverage::" << model_grids.at(best_grid).max_coverage << " +Max::" << model_grids.at(best_grid).max_pos << " -Max::" << model_grids.at(best_grid).max_neg 
							<< " DiffMin::" << model_grids.at(best_grid).min_diff << " BestRot::" << model_grids.at(best_grid).best_rot << " BestMinUVWT::" << model_grids.at(best_grid).best_min_uvw_t.ToString() << " BestMaxUVWT::" << 
							model_grids.at(best_grid).best_max_uvw_t.ToString() << std::endl;
						std::cout << model_grids.at(best_grid).vgrid->ToString();				
				

						/*
						//Use this code to view voxel grids rendered directly in the current scene 
						//FeatureDescStructureGraph::VoxelGrid * voxel_grid;				
						currentlyViewedModelVG = 6;
						crt_model_grid = model_grids.at(currentlyViewedModelVG).tgrid;
						std::cout << "Using grid ID::" << currentlyViewedModelVG << std::endl;
						//std::vector<int> segs; segs.push_back(boost::lexical_cast<int, std::string>(visibility_segment_number));					
							
						pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(segs, crt_model_grid, 1, grid_rot, Vector3(u_min_t, v_min_t, w_min_t), Vector3(u_max_t, v_max_t, w_max_t), voxel_grid);
						voxel_grid_measure = crt_model_grid->ComputeOverlapDiffWithGridUVW(voxel_grid);					
						coverage = crt_model_grid->ComputeOverlapPosWithGridUVW(voxel_grid);																
				
						if (!all_points_in_grid) 
						{
							all_points_in_grid = true;					
							if (voxel_grid->above_u_max > 0 | voxel_grid->below_u_min > 0) { u_max_t += t_step; u_min_t += t_step; all_points_in_grid = false; std::cout << " U++ Above::" << voxel_grid->above_u_max << " Below::" << voxel_grid->below_u_min << std::endl; }					
							if (voxel_grid->below_v_min > 0) { v_min_t += t_step; all_points_in_grid = false; std::cout << " BelowV::" << voxel_grid->below_v_min << std::endl; }
							if (voxel_grid->above_v_max > 0) { v_max_t += t_step; all_points_in_grid = false; std::cout << " AboveV::" << voxel_grid->above_v_max << std::endl; }
							if (voxel_grid->below_w_min > 0) { w_min_t += t_step; all_points_in_grid = false; std::cout << " BelowW::" << voxel_grid->below_w_min << std::endl; }
							if (voxel_grid->above_w_max > 0) { w_max_t += t_step; all_points_in_grid = false; std::cout << " AboveW::" << voxel_grid->above_w_max << std::endl; }
						} 
						else
						{					
							grid_rot += Maths::PiTwo / 24;
							u_min_t = 0; v_min_t = 0; w_min_t = 0;
							u_max_t = 0; v_max_t = 0; w_max_t = 0;
							all_points_in_grid = false;
							//currentlyViewedModelVG = (currentlyViewedModelVG+1) % model_grids.size();
							//std::cout << "Next Grid :: " << currentlyViewedModelVG << std::endl;
							std::cout << "Reset Volume and Apply Rotation:: " << grid_rot << std::endl;
						}		
						*/
						//*****
						std::vector<std::pair<std::pair<bool, float>, Vector3>> * grid_spheres;		
						//grid_spheres = voxel_grid->GetActiveOverlappingCells(crt_model_grid);
						grid_spheres = crt_model_grid->GetAllCellsShowingActive(voxel_grid);
						std::cout << "\nCrt Coverage::" << coverage << " PosMatches::" << voxel_grid_measure.second.first << " NegMatches::" << voxel_grid_measure.second.second << " Diffs::" << voxel_grid_measure.first << " Number of spheres::" << grid_spheres->size() << std::endl;
						KDTreeSphereMesh * sphereMesh = new KDTreeSphereMesh("scene_seg_"+visibility_segment_number+"_grid");
						int g = 0;
						for (auto s : *grid_spheres)
						{
							if (s.first.first) g = 0; else g = 1;
							sphereMesh->AddSphere(s.second, s.first.second, g);
						}
						sphereMesh->ComputeBoundingVolume();				
						sphereMesh->Compile();
						gp->SetShape(sphereMesh);
						gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
						gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid("scene_seg_:"+visibility_segment_number, gp);
						scene_change = true;
						accuReset = true;				
					}
				}

				//Initiate Building of MDP space.
				if (processing_search_request && key.GetKeyState(VK_F1) == KeyboardStates::clicked)
				{
					accuReset = true;
					std::cout << "[Main] Computing Search Space for Structure Fitting - MDP 1 - [START]" << std::endl;
					pcs->ComputeSearchSpace();
					std::vector<FeatureDescStructureGraph::VG_stats*> * vgs = pcs->m_sceneSolution->m_markovDecisionProcess->GetVG_LeafFirstTraversal();
					int idx=0;
			
					for (auto vg : *vgs)
					{
						pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(vg->segs, vg->tgrid, 1, vg->best_rot, vg->best_min_uvw_t, vg->best_max_uvw_t, vg->vgrid);
						std::vector<std::pair<std::pair<bool, float>, Vector3>> * grid_spheres;
						//grid_spheres = vg->vgrid->GetActiveOverlappingCellsWithGridBoundaries(vg->tgrid);
						grid_spheres = vg->tgrid->GetAllCellsShowingActive(vg->vgrid);
						std::cout << vg->tgrid->m_parent->GetInitialNodeId() << ")) Coverage::" << vg->max_coverage << " PosMatches::" << vg->max_pos << " NegMatches::" << vg->max_neg << " Diffs::" << vg->min_diff << " Number of spheres::" << grid_spheres->size() << std::endl;
						//std::cout << "V:" << vg->vgrid->ToString() << std::endl;
						//std::cout << "T:" << vg->tgrid->ToString() << std::endl;
						KDTreeSphereMesh * sphereMesh = new KDTreeSphereMesh("scene_obj_grid_"+idx); idx++;
						int g = 0;
						for (auto s : *grid_spheres)
						{
							if (s.first.first) g = 0; else g = 1;
							sphereMesh->AddSphere(s.second, s.first.second, g);
						}
						sphereMesh->ComputeBoundingVolume();
						sphereMesh->Compile();
						GeometricPrimitive * gp = new GeometricPrimitive();
						gp->SetShape(sphereMesh);
						gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
						gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
						//((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid("scene_obj_:"+idx, gp);

						//Add Model associated to basicspace_pcs.
						std::pair<bool, KDTreeMesh*> shape = pcs->m_trainedModelsDB->GetShape(boost::lexical_cast<std::string, int>(vg->tgrid->m_parent->m_pcs->m_id));
						if (shape.first)
						{				
							GeometricPrimitive * gp1 = new GeometricPrimitive();
							gp1->SetShape(shape.second);					
							gp1->WorldTransform.Reset();

							Matrix3x3 uvw_inverse_scene = vg->vgrid->uvw.GetMatrix();
							uvw_inverse_scene.Transpose();

							Matrix3x3 uvw_inverse_model = vg->tgrid->uvw.GetMatrix();
							uvw_inverse_model.Transpose();					

							Vector3 min_scene_xyz; Vector3 max_scene_xyz;
							Matrix3x3::Product(uvw_inverse_scene, vg->vgrid->min_model_uvw, min_scene_xyz);
							Matrix3x3::Product(uvw_inverse_scene, vg->vgrid->max_model_uvw, max_scene_xyz);
							min_scene_xyz+=vg->vgrid->o_model_xyz;
							max_scene_xyz+=vg->vgrid->o_model_xyz;					

							Vector3 min_model_xyz; Vector3 max_model_xyz;
							Matrix3x3::Product(uvw_inverse_model, vg->tgrid->min_model_uvw, min_model_xyz);
							Matrix3x3::Product(uvw_inverse_model, vg->tgrid->max_model_uvw, max_model_xyz);
							min_model_xyz+=vg->tgrid->o_model_xyz;
							max_model_xyz+=vg->tgrid->o_model_xyz;

							Vector3 segment_origin_model = vg->tgrid->o_model_xyz;
							Vector3 segment_origin_scene = vg->vgrid->o_model_xyz;

							uvw_inverse_scene.Transpose();
							uvw_inverse_model.Transpose();
					
							float scale_factor = Vector3::Distance(max_scene_xyz,min_scene_xyz) / Vector3::Distance(max_model_xyz,min_model_xyz);
							float scale_factor_u = vg->vgrid->grid_cell_spacing_u / vg->tgrid->grid_cell_spacing_u;
							float scale_factor_v = vg->vgrid->grid_cell_spacing_v / vg->tgrid->grid_cell_spacing_v;
							float scale_factor_w = vg->vgrid->grid_cell_spacing_w / vg->tgrid->grid_cell_spacing_w;
							Vector3 scale_vector(scale_factor, scale_factor, scale_factor);

							Vector3 scale_vector_r(1/scale_factor_u, 1/scale_factor_v, 1/scale_factor_w);

							min_model_xyz *= scale_vector;
							max_model_xyz *= scale_vector;
							Vector3 translation = segment_origin_scene; // - segment_origin_model;

							Vector3 max_min_orientation_model = Vector3::Normalize(max_model_xyz - min_model_xyz);
							Vector3 max_min_orientation_scene = Vector3::Normalize(max_scene_xyz - min_scene_xyz);

							std::vector<Vector3> basis = vg->vgrid->GetBasisVectorsInWorldSpace();
							Vector3 min_loc = basis.at(0);
							Vector3 u_direction_s = Vector3::Normalize(basis.at(1)-min_loc);
							Vector3 v_direction_s = Vector3::Normalize(basis.at(2)-min_loc);
							Vector3 w_direction_s = Vector3::Normalize(basis.at(3)-min_loc);
					
							std::vector<Vector3> basis_m = vg->tgrid->GetBasisVectorsInWorldSpace();
							Vector3 min_loc_m = basis_m.at(0);
							Vector3 u_direction_m = Vector3::Normalize(basis_m.at(1)-min_loc_m);
							Vector3 v_direction_m = Vector3::Normalize(basis_m.at(2)-min_loc_m);
							Vector3 w_direction_m = Vector3::Normalize(basis_m.at(3)-min_loc_m);
					
							float rot_angle_u = Maths::Acos(Vector3::Dot(u_direction_s, u_direction_m));
							Vector3 rot_vector_u = Vector3::Normalize(Vector3::Cross(u_direction_m, u_direction_s));

							float rot_angle_v = Maths::Acos(Vector3::Dot(v_direction_s, v_direction_m));
							Vector3 rot_vector_v = Vector3::Normalize(Vector3::Cross(v_direction_m, v_direction_s));

							float rot_angle_w = Maths::Acos(Vector3::Dot(w_direction_s, w_direction_m));
							Vector3 rot_vector_w = Vector3::Normalize(Vector3::Cross(w_direction_m, w_direction_s));

							translation = segment_origin_scene - segment_origin_model;

							Matrix4x4 tr = Matrix4x4::CreateTranslation(translation);
							Matrix4x4 sc = Matrix4x4::CreateScaling(scale_vector);
					
							Matrix3x3 ro_u_ = Matrix3x3::CreateRotation(rot_vector_u, rot_angle_u);
					
							Transformation ru; ru.Reset(); ru.SetRotation(ro_u_); 
							Vector3 transformed_v = ru.Apply(v_direction_m);
							float angle_vv =  Maths::Acos(Vector3::Dot(transformed_v, v_direction_s));										

							Matrix3x3 ro_v_ = Matrix3x3::CreateRotation(u_direction_s, -angle_vv);										

							Transformation rv; rv.Reset(); rv.SetRotation(ro_v_);
					
							Vector3 transformed_v2 = rv.Apply(transformed_v);
							transformed_v2.Normalize();					 

							Vector3 cross_u_v = Vector3::Cross(u_direction_s, transformed_v2);

							if (Vector3::Dot(cross_u_v, w_direction_s) < 0)	ro_v_ = Matrix3x3::CreateRotation(u_direction_s, angle_vv);					

							Matrix4x4 ro_u = Matrix4x4(ro_u_);
							Matrix4x4 ro_v = Matrix4x4(ro_v_);
										
							Matrix4x4 tr_0r = Matrix4x4::CreateTranslation(-segment_origin_model);
							Matrix4x4 tr_1r = Matrix4x4::CreateTranslation(segment_origin_model);
					
							//----
							KDTreeSphereMesh * sphereMesh_ = new KDTreeSphereMesh("scene_obj_grid_"+idx); idx++;
							//sphereMesh_->AddSphere(min_model_xyz, 1.1, 1);
							//sphereMesh_->AddSphere(max_model_xyz, 1.5, 0);
					
							//sphereMesh_->AddSphere(min_scene_xyz, 1.1, 1);
							//sphereMesh_->AddSphere(max_scene_xyz, 1.5, 0);					

							//std::cout << "O:" << min_loc.ToString() << " U:" << u_direction_s.ToString() << std::endl; // " W:" << w_direction.ToString() << std::endl;
					
							/*sphereMesh_->AddSphere(segment_origin_scene+u_direction_s, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*2, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*3, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*4, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*5, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*6, 0.4, 1);
					
							sphereMesh_->AddSphere(segment_origin_scene+v_direction_s, 0.4, 0);
							sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*2, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*3, 0.4, 0);
							sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*4, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*5, 0.4, 0);
							sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*6, 0.4, 1);					

							sphereMesh_->AddSphere(segment_origin_model+u_direction_m, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_model+u_direction_m*2, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_model+u_direction_m*3, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_model+u_direction_m*4, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_model+u_direction_m*5, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_model+u_direction_m*6, 0.4, 1);
					
							sphereMesh_->AddSphere(segment_origin_model+v_direction_m, 0.4, 0);
							sphereMesh_->AddSphere(segment_origin_model+v_direction_m*2, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_model+v_direction_m*3, 0.4, 0);
							sphereMesh_->AddSphere(segment_origin_model+v_direction_m*4, 0.4, 1);
							sphereMesh_->AddSphere(segment_origin_model+v_direction_m*5, 0.4, 0);
							sphereMesh_->AddSphere(segment_origin_model+v_direction_m*6, 0.4, 1);
							*/
							/*sphereMesh_->AddSphere(min_scene_xyz+v_direction, 0.6, 0);
							sphereMesh_->AddSphere(min_scene_xyz+v_direction*2, 0.6, 0);
							sphereMesh_->AddSphere(min_scene_xyz+v_direction*3, 0.6, 0);
							sphereMesh_->AddSphere(min_scene_xyz+v_direction*4, 0.6, 0);

							sphereMesh_->AddSphere(min_scene_xyz+w_direction, 0.8, 1);
							sphereMesh_->AddSphere(min_scene_xyz+w_direction*2, 0.8, 1);
							sphereMesh_->AddSphere(min_scene_xyz+w_direction*3, 0.8, 1);
							sphereMesh_->AddSphere(min_scene_xyz+w_direction*4, 0.8, 1);*/

							//sphereMesh_->AddSphere(Vector3(0,0,0), 0.5, 1);
							/*sphereMesh_->AddSphere(Vector3(1,0,0), 0.5, 1);
							sphereMesh_->AddSphere(Vector3(2,0,0), 0.5, 0);
							sphereMesh_->AddSphere(Vector3(3,0,0), 0.5, 1);
							sphereMesh_->AddSphere(Vector3(4,0,0), 0.5, 0);
							sphereMesh_->AddSphere(Vector3(0,1,0), 0.5, 1);
							sphereMesh_->AddSphere(Vector3(0,2,0), 0.5, 1);
							sphereMesh_->AddSphere(Vector3(0,3,0), 0.5, 1);
							sphereMesh_->AddSphere(Vector3(0,4,0), 0.5, 1);
							sphereMesh_->AddSphere(Vector3(0,0,1), 0.5, 0);
							sphereMesh_->AddSphere(Vector3(0,0,2), 0.5, 0);
							sphereMesh_->AddSphere(Vector3(0,0,3), 0.5, 0);
							sphereMesh_->AddSphere(Vector3(0,0,4), 0.5, 0);*/
							//sphereMesh_->AddSphere(segment_origin_model, 1.0, 1);
							//sphereMesh_->AddSphere(segment_origin_scene, 1.4, 0);
					
							//-------
					
							Matrix4x4 m1, m2, m3, comp;
							((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform.GetTransform(m1);
							gp1->WorldTransform.GetTransform(m2);

							Matrix4x4 worldRotation(((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform.GetRotation());
					
							comp = (tr_1r * ((ro_v * ro_u) * (sc * tr_0r)));					
						
							Matrix4x4 allRotations = worldRotation * ro_v * ro_u;				
							Matrix4x4 ro_v_u = ro_v * ro_u;					

							m3 = worldRotation;

							gp1->WorldTransform.Reset();
							float r[9] = {m3._00, m3._01, m3._02, m3._10, m3._11, m3._12, m3._20, m3._21, m3._22};
							gp1->WorldTransform.SetRotation(Matrix3x3(r));
							gp1->WorldTransform.SetTranslation(Vector3(m3._03, m3._13, m3._23));
					
							Transformation rv2;
							rv2.Reset();
							float r_[9] = {ro_v_u._00, ro_v_u._01, ro_v_u._02, ro_v_u._10, ro_v_u._11, ro_v_u._12, ro_v_u._20, ro_v_u._21, ro_v_u._22};
							rv2.SetRotation(Matrix3x3(r_));
							rv2.SetScaling(scale_vector);
							rv2.SetTranslation(Vector3(0,0,0));
							Vector3 transformed_model_origin = rv2.Apply(segment_origin_model);
					
							for (int i=0; i<shape.second->VertexList.Size(); i++) shape.second->VertexList[i].Position = rv2.Apply(shape.second->VertexList[i].Position);
							for (int i=0; i<shape.second->VertexList.Size(); i++) shape.second->VertexList[i].Position+=segment_origin_scene-transformed_model_origin;
							for (int i = 0; i<shape.second->TriangleList.Size(); i++) { shape.second->TriangleList[i].ComputeEdges(); }					
							shape.second->ComputeBoundingVolume();
							shape.second->Compile();					
					
							sphereMesh_->AddSphere(segment_origin_scene, 1.5, 1);
							//sphereMesh_->AddSphere(transformed_model_origin, 1.0, 1);										

							gp1->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("LitMaterial"));
							std::cout << "Material Name::" << gp1->GetMaterial()->GetName() << std::endl;
							std::string name = "obj_"+boost::lexical_cast<std::string, int>(vg->segs.front());
							((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddModel(name, gp1, tr, ro_u, sc, max_min_orientation_model, comp, tr_0r, tr_1r, scale_vector, translation);

							sphereMesh_->ComputeBoundingVolume();
							sphereMesh_->Compile();
							GeometricPrimitive * gp3 = new GeometricPrimitive();
							gp3->SetShape(sphereMesh_);
							gp3->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
							gp3->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
							((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid("spheregrid:"+name, gp3);
						}
					}
			
					scene_change = true;
					accuReset = true;	
					std::cout << "[Main] Computing Search Space for Structure Fitting - MDP 1 - [FINISH]" << std::endl;
					processing_search_request = false;			
				}
					
				//Reset and show everything
				if (processing_visibility_request && key.GetKeyState(VK_F1) == KeyboardStates::clicked)
				{						
					accuReset = true;
					std::cout << "[Main] Setting All Points to Visible." << std::endl;
					((KDTreePointCloud*)(gp->GetShape()))->SetVisibleAllPoints();
					processing_visibility_request = false;
					accuReset=true;
				}

				//Hide everything
				if (processing_visibility_request && key.GetKeyState(VK_F2) == KeyboardStates::clicked)
				{					
					accuReset = true;
					std::cout << "[Main] Setting All Points to Invisible." << std::endl;
					((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPoints();
					processing_visibility_request = false;			
				}

				//Show Scene OBB
				if (processing_visibility_request && key.GetKeyState(VK_F3) == KeyboardStates::clicked)
				{			
					GeometricPrimitive * gp = new GeometricPrimitive();
					KDTreeMesh * q_mesh = new KDTreeMesh();			
					pcs->m_sceneToSolve->ComputeOBB(0.05f);
					ShapeForge::CreateOBBWireFrame(pcs->m_sceneToSolve->m_obb.obb_0, pcs->m_sceneToSolve->m_obb.obb_1, pcs->m_sceneToSolve->m_obb.obb_2, pcs->m_sceneToSolve->m_obb.p_0_min.Distance, pcs->m_sceneToSolve->m_obb.p_0_max.Distance,
																																				pcs->m_sceneToSolve->m_obb.p_1_min.Distance, pcs->m_sceneToSolve->m_obb.p_1_max.Distance,
																																				pcs->m_sceneToSolve->m_obb.p_2_min.Distance, pcs->m_sceneToSolve->m_obb.p_2_max.Distance, q_mesh);
						
					q_mesh->ComputeBoundingVolume();
					q_mesh->Compile();
					gp->SetShape(q_mesh);
					gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
					gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
			
					//pcs->m_pEnvironment->GetSpace()->PrimitiveList.PushBack(gp);
					((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("scene", gp);

					std::cout << "OBB_U MaxDist = " << pcs->m_sceneToSolve->m_obb.p_0_max.Distance << " MinDist=" << pcs->m_sceneToSolve->m_obb.p_0_min.Distance << " N=" << pcs->m_sceneToSolve->m_obb.obb_0.ToString() << std::endl;
					std::cout << "OBB_V MaxDist = " << pcs->m_sceneToSolve->m_obb.p_1_max.Distance << " MinDist=" << pcs->m_sceneToSolve->m_obb.p_1_min.Distance << " N=" << pcs->m_sceneToSolve->m_obb.obb_1.ToString() << std::endl;
					std::cout << "OBB_W MaxDist = " << pcs->m_sceneToSolve->m_obb.p_2_max.Distance << " MinDist=" << pcs->m_sceneToSolve->m_obb.p_2_min.Distance << " N=" << pcs->m_sceneToSolve->m_obb.obb_2.ToString() << std::endl;

					processing_visibility_request = false;
				}

				//Show OBBS of all surface segments in the scene
				if (processing_visibility_request && key.GetKeyState(VK_F9) == KeyboardStates::clicked)
				{
					try {
						accuReset = true;
						int obbcount = 0;
						processing_visibility_request = false;				
						SegmentsGraph * sg = pcs->m_sceneToSolve->GetSegmentsGraph();
						for (auto seg_it = pcs->m_sceneToSolve->GetStructureGraph()->m_nodes->begin(); seg_it != pcs->m_sceneToSolve->GetStructureGraph()->m_nodes->end(); seg_it++)
						{
							std::string segment_id = boost::lexical_cast<std::string, int>(seg_it->first);
							if (!((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+segment_id, true))
							{
								GeometricPrimitive * gp = new GeometricPrimitive();
								KDTreeMesh * q_mesh = new KDTreeMesh();
								ShapeForge::CreateOBBWireFrame(sg->m_surfaceSegments->at(seg_it->first)->obb.obb_0, sg->m_surfaceSegments->at(seg_it->first)->obb.obb_1, sg->m_surfaceSegments->at(seg_it->first)->obb.obb_2,
																sg->m_surfaceSegments->at(seg_it->first)->obb.p_0_min.Distance, sg->m_surfaceSegments->at(seg_it->first)->obb.p_0_max.Distance,
																sg->m_surfaceSegments->at(seg_it->first)->obb.p_1_min.Distance, sg->m_surfaceSegments->at(seg_it->first)->obb.p_1_max.Distance,
																sg->m_surfaceSegments->at(seg_it->first)->obb.p_2_min.Distance, sg->m_surfaceSegments->at(seg_it->first)->obb.p_2_max.Distance, q_mesh);
						
								q_mesh->ComputeBoundingVolume();
								q_mesh->Compile();
								gp->SetShape(q_mesh);
								gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
								gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
												
								((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("S"+segment_id, gp);
								obbcount++;
							}
						}
						std::cout << "[Main] Showing " << obbcount << " OBBs of ALL Surface Segments"<< std::endl;

					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
					}
				}

				//Hide all OBBs
				if (processing_visibility_request && key.GetKeyState(VK_F10) == KeyboardStates::clicked)
				{
					try {				
						accuReset = true;				
						processing_visibility_request = false;
						std::cout << "[Main] Hiding OBBs of ALL Surface Segments"<< std::endl;											
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleAllOBBs(false);

					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
					}
				}

				if (processing_visibility_request && key.GetKeyState(VK_F11) == KeyboardStates::clicked)
				{

				}

				if (processing_visibility_request && key.GetKeyState(VK_F12) == KeyboardStates::clicked)
				{

				}		

				//Show Surface Points
				if (processing_visibility_request && key.GetKeyState(VK_F5) == KeyboardStates::clicked)
				{					
					accuReset = true;			
					((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithTypeId(4);
					processing_visibility_request = false;			
				}

				//Show Edge Points
				if (processing_visibility_request && key.GetKeyState(VK_F6) == KeyboardStates::clicked)
				{					
					accuReset = true;			
					((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithTypeId(3);
					processing_visibility_request = false;
				}

				//Show Corner Points
				if (processing_visibility_request && key.GetKeyState(VK_F7) == KeyboardStates::clicked)
				{					
					accuReset = true;			
					((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithTypeId(5);
					processing_visibility_request = false;
				}

				//Show Seed Points
				if (processing_visibility_request && key.GetKeyState(VK_F8) == KeyboardStates::clicked)
				{					
					accuReset = true;			
					((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptSeeds();
					processing_visibility_request = false;
				}		

				//View Corner Detection Process
				if (processing_visibility_request && key.GetKeyState('C') == KeyboardStates::clicked)
				{				
					accuReset = true;
					((KDTreePointCloud*)(gp->GetShape()))->ComputeCornerTypes(std::vector<int>(), true);
					processing_visibility_request = false;
				}
				
				//View All Points in One Colour
				if (processing_visibility_request && key.GetKeyState('R') == KeyboardStates::clicked)
				{
					accuReset = true;
					if (restore)
					{
						((KDTreePointCloud*)(gp->GetShape()))->RestoreAllPointsSegmentIdsFromCache();
						restore = false;
					}
					else
					{
						((KDTreePointCloud*)(gp->GetShape()))->SetAllPointsInOneSegment();
						restore = true;
					}
					processing_visibility_request = false;
				}

				//Switch State and wait for input of surface segment number
				if (processing_visibility_request && !processing_visibility_segmentid && key.GetKeyState('S') == KeyboardStates::clicked)
				{							
					processing_visibility_segmentid = true;
					std::cout << "[Main] Input Segment Number ... " << std::endl;
					visibility_segment_number.clear();
				}

				//Switch State and wait for input of OBB segment number 
				if (processing_visibility_request && !processing_visibility_obbsegmentid && key.GetKeyState('O') == KeyboardStates::clicked)
				{							
					processing_visibility_obbsegmentid = true;
					std::cout << "[Main] Input OBB Segment Number ... " << std::endl;
					visibility_segment_number.clear();
				}

				//Show ONLY points with segmentid
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('S') == KeyboardStates::clicked)
				{					
					try {
						//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
						accuReset = true;
						processing_visibility_segmentid = false;											
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
						if (chosen_segment != NULL)
						{					
							std::cout << "[Main] Showing Surface Segment with id " << segment_id << std::endl;
							((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
							currentlyViewedSegment = segment_id;
							chosen_segment->internalGraphSelectionIndex = -1;							
						} 
						else
						{
							SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
							if (chosen_edge_segment != NULL)
							{
								std::cout << "[Main] Showing Edge Segment with id " << segment_id << std::endl;			
								((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
								//currentlyViewedSegment = segment_id;
								//chosen_segment->internalGraphSelectionIndex = -1;							
							}
							std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
						}

					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
					} 

				}

				//Show OBB of segment 
				if (processing_visibility_request && processing_visibility_obbsegmentid && key.GetKeyState('O') == KeyboardStates::clicked)
				{					
					try {
						//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
						accuReset = true;
						processing_visibility_obbsegmentid = false;
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
						if (chosen_segment != NULL)
						{
							std::cout << "[Main] Showing OBB of Surface Segment with id " << segment_id << std::endl;
							if (!((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+visibility_segment_number, true))
							{
								GeometricPrimitive * gp = new GeometricPrimitive();
								KDTreeMesh * q_mesh = new KDTreeMesh();
								ShapeForge::CreateOBBWireFrame(chosen_segment->obb.obb_0, chosen_segment->obb.obb_1, chosen_segment->obb.obb_2, chosen_segment->obb.p_0_min.Distance, chosen_segment->obb.p_0_max.Distance,
																																				chosen_segment->obb.p_1_min.Distance, chosen_segment->obb.p_1_max.Distance,
																																				chosen_segment->obb.p_2_min.Distance, chosen_segment->obb.p_2_max.Distance, q_mesh);
						
								std::cout << "OBB_U MaxDist = " << chosen_segment->obb.p_0_max.Distance << " MinDist=" << chosen_segment->obb.p_0_min.Distance << " N=" << chosen_segment->obb.obb_0.ToString() << std::endl;
								std::cout << "OBB_V MaxDist = " << chosen_segment->obb.p_1_max.Distance << " MinDist=" << chosen_segment->obb.p_1_min.Distance << " N=" << chosen_segment->obb.obb_1.ToString() << std::endl;
								std::cout << "OBB_W MaxDist = " << chosen_segment->obb.p_2_max.Distance << " MinDist=" << chosen_segment->obb.p_2_min.Distance << " N=" << chosen_segment->obb.obb_2.ToString() << std::endl;
								q_mesh->ComputeBoundingVolume();
								q_mesh->Compile();
								gp->SetShape(q_mesh);
								gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
								gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
												
								((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("S"+visibility_segment_number, gp);

							}
						}
						else
						{
							SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
							if (chosen_edge_segment != NULL)
							{
								std::cout << "[Main] NO IMPLEMENTATION - Showing OBB of Edge Segment with id " << segment_id << std::endl;								
					
							}
							std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
						}

					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
					} 
				}

				//Show OBB of internal nodes in segment 
				if (processing_visibility_request && processing_visibility_obbsegmentid && key.GetKeyState('I') == KeyboardStates::clicked)
				{					
					try {
						//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
						accuReset = true;
						processing_visibility_obbsegmentid = false;											
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				
						if (chosen_segment != NULL)
						{				
							if (chosen_segment->shape_primitive != SegmentsGraph::compound) 
							{
								std::cout << "[Main] Primitive of Surface Segment with id " << segment_id << " is not compound" << std::endl;						
							}
							else
							{
								std::cout << "[Main] Showing OBB of Internal Segments for Surface Segment with id " << segment_id << std::endl;
								StructureGraph * internalStrGraph = chosen_segment->internalStrGraph;
								SegmentsGraph * internalSegGraph = chosen_segment->internalSegGraph;
								for (auto nodes_it = internalStrGraph->m_nodes->begin(); nodes_it != internalStrGraph->m_nodes->end(); nodes_it++)
								{
									if (internalStrGraph->IsNodeMerged(nodes_it->first)) continue;
									std::string node_id = boost::lexical_cast<std::string, int>(nodes_it->first);
									if (!((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+visibility_segment_number+"I"+node_id, true))
									{
										GeometricPrimitive * gp = new GeometricPrimitive();
										KDTreeMesh * q_mesh = new KDTreeMesh();						
										ShapeForge::CreateOBBWireFrame(internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.obb_0, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.obb_1, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.obb_2, 
																		internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_0_min.Distance, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_0_max.Distance,
																		internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_1_min.Distance, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_1_max.Distance,
																		internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_2_min.Distance, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_2_max.Distance, q_mesh);
						
										q_mesh->ComputeBoundingVolume();
										q_mesh->Compile();
										gp->SetShape(q_mesh);
										gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
										gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
												
										((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("S"+visibility_segment_number+"I"+node_id, gp);
								}
								}
							}
						} 				

					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
					} 
				}

				//Hide OBB of segment 
				if (processing_visibility_request && processing_visibility_obbsegmentid && key.GetKeyState('H') == KeyboardStates::clicked)
				{					
					try {
						//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
						accuReset = true;
						processing_visibility_obbsegmentid = false;											
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
						if (chosen_segment != NULL)
						{					
							std::cout << "[Main] Hiding OBB of Surface Segment with id " << segment_id << std::endl;			
							((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+visibility_segment_number, false);					
						} 
						else
						{
							SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
							if (chosen_edge_segment != NULL)
							{
								std::cout << "[Main] NO IMPLEMENTATION - Showing OBB of Edge Segment with id " << segment_id << std::endl;								
					
							}
							std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
						}

					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
					} 

				}

				//Show ONLY points of the mostly visible surface
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('O') == KeyboardStates::clicked)
				{					
					try {
						//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
						accuReset = true;
						processing_visibility_segmentid = false;											
						processing_visibility_request = false;
						int segment_id = pcs->m_sceneToSolve->m_mostVisibileSurface;
						std::cout << "[Main] Max Visible Surface (MAX VisibilitY) set to id " << segment_id << std::endl;
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
						if (chosen_segment != NULL)
						{					
							std::cout << "[Main] Showing Surface Segment (MAX VisibilitY) with id " << segment_id << std::endl;
							((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
							currentlyViewedSegment = segment_id;
							chosen_segment->internalGraphSelectionIndex = -1;							
						} 
						else
						{
							SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
							if (chosen_edge_segment != NULL)
							{
								std::cout << "[Main] Showing Edge Segment with id " << segment_id << std::endl;			
								((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
								//currentlyViewedSegment = segment_id;
								//chosen_segment->internalGraphSelectionIndex = -1;							
							}
							std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
						}

					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
					} 

				}

				//Hide points with segmentid
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('H') == KeyboardStates::clicked)
				{					
					try {				
						accuReset = true;
						processing_visibility_segmentid = false;											
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
						if (chosen_segment != NULL)
						{
							std::cout << "[Main] Not Showing Segment with id " << segment_id << std::endl;			
							((KDTreePointCloud*)(gp->GetShape()))->SetInvisiblePointsWithSegmentId(segment_id);							
						}
						else
						{
							std::cout << "[EXCEPTION THROWN <H>] Segment not found :" << visibility_segment_number << std::endl;
						}
					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <H>] Boost::Bad_lexical_cast" << std::endl;
					}
				}

				//Show also points with segmentid
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('A') == KeyboardStates::clicked)
				{					
					try {				
						accuReset = true;
						processing_visibility_segmentid = false;											
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
						if (chosen_segment != NULL)
						{
							std::cout << "[Main] Adding Segment with id " << segment_id << std::endl;			
							((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentId(segment_id);							
						}
						else
						{
							std::cout << "[EXCEPTION THROWN <A>] Segment not found :" << visibility_segment_number << std::endl;
						}
					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <A>] Boost::Bad_lexical_cast" << std::endl;
					}
				}

				//Show directly connected segments
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('D') == KeyboardStates::clicked)
				{								
					try {
						accuReset = true;
						processing_visibility_segmentid = false;
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
						if (chosen_segment != NULL)
						{					
							std::vector<int> connected_segments = pcs->m_sceneToSolve->GetStructureGraph()->DirectlyConnectedSegmentsAtAngle(segment_id, 0.95f, 0.05f);			
							std::cout << "[Main] Showing " << connected_segments.size() << " segments connected DIRECTLY (at angle) with segment # " << segment_id << std::endl;			
							((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentIds(connected_segments);
							currentlyViewedSegment = segment_id;
							chosen_segment->internalGraphSelectionIndex = -1;							
						}
						else
						{
							std::cout << "[EXCEPTION THROWN <D>] Segment not found :" << visibility_segment_number << std::endl;
						}
					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <D>] Boost::Bad_lexical_cast" << std::endl;
					}
				}

				//Merge Surface States
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('M') == KeyboardStates::clicked)
				{					
					try {
						accuReset = true;
						processing_visibility_segmentid = false;
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
						if (chosen_segment != NULL)
						{
							bool done = false;
							//Check if we first have to merge the internal states of this segment
							if (chosen_segment->internalSegGraph == NULL)
							{
								pcs->m_sceneToSolve->MergeStates(true, segment_id, 2, 0.95f, 0.05f, 0);			
								std::cout << "[Main] Merging (Level 0) segments connected DIRECTLY (at angle) with segment #" << segment_id << std::endl;			
								currentlyViewedSegment = segment_id;
								chosen_segment->internalGraphSelectionIndex = -1;
								done = true;
							}

							if (!done)
							{
								if (chosen_segment->internalSegGraph->m_surfaceSegments->size() > 1)
								{
									pcs->m_sceneToSolve->MergeStates(true, segment_id, 2, 0.95f, 0.05f, 1);			
									std::cout << "[Main] Merging (Level 1) segments connected DIRECTLY (at angle) in segment #" << segment_id << std::endl;
									currentlyViewedSegment = segment_id;
									chosen_segment->internalGraphSelectionIndex = -1;
								}
								else
								{
									pcs->m_sceneToSolve->MergeStates(true, segment_id, 2, 0.95f, 0.05f, 0);			
									std::cout << "[Main] Merging (Level 0) segments connected DIRECTLY (at angle) with segment #" << segment_id << std::endl;			
									currentlyViewedSegment = segment_id;
									chosen_segment->internalGraphSelectionIndex = -1;
								}
							}
						}
						else
						{
							std::cout << "[EXCEPTION THROWN <M>] Segment not found :" << visibility_segment_number << std::endl;
						}
					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <M>] Boost::Bad_lexical_cast" << std::endl;
					}
				}

				//Use G to merge automatically global graph
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('G') == KeyboardStates::clicked)
				{					
					try
					{
						accuReset = true;
						processing_visibility_segmentid = false;
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
						if (chosen_segment != NULL)
						{
							std::cout << "[Main] Merging (AUTO) level 0 segments connected DIRECTLY (at angle) to segment " << segment_id << std::endl;
							pcs->m_sceneToSolve->MergeStates(false, segment_id, 2, 0.95f, 0.05f, 0);
						}
						else
						{
							std::cout << "[EXCEPTION THROWN <M>] Segment not found :" << visibility_segment_number << std::endl;
						}
					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN <M>] Boost::Bad_lexical_cast" << std::endl;
					}
				}

				//Show all directly connected surfaces
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('F') == KeyboardStates::clicked)
				{					
					try {
						accuReset = true;
						processing_visibility_segmentid = false;				
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
						if (chosen_segment != NULL)
						{
							std::vector<int> connected_segments = pcs->m_sceneToSolve->GetStructureGraph()->AllDirectlyConnectedSegments(segment_id);
							std::cout << "[Main] Showing " << connected_segments.size() << " segments connected DIRECTLY with segment # " << segment_id << std::endl;
							((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentIds(connected_segments);
							currentlyViewedSegment = segment_id;
							chosen_segment->internalGraphSelectionIndex = -1;
						}
					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN] Boost::Bad_lexical_cast" << std::endl;
					}
				}

				//Show transitively connected surfaces at angle
				if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('T') == KeyboardStates::clicked)
				{					
					try {
						accuReset = true;
						processing_visibility_segmentid = false;											
						processing_visibility_request = false;
						int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);			
						SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
						if (chosen_segment != NULL)
						{
							std::vector<int> connected_segments = pcs->m_sceneToSolve->GetStructureGraph()->TransitivelyConnectedSegmentsAtAngle(segment_id, 0.95f, 0.05f);
							std::cout << "[Main] Showing " << connected_segments.size() << " segments connected TRANSITIVELY (at angle) with segment # " << segment_id << std::endl;
							((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentIds(connected_segments);
							currentlyViewedSegment = segment_id;
							chosen_segment->internalGraphSelectionIndex = -1;							
						}
					} catch (boost::bad_lexical_cast) {
						std::cout << "[EXCEPTION THROWN] Boost::Bad_lexical_cast" << std::endl;
					}
				}		

				if (processing_visibility_request && (processing_visibility_segmentid || processing_visibility_obbsegmentid))
				{
					if (key.GetKeyState('0') == KeyboardStates::clicked) visibility_segment_number.append("0");
					if (key.GetKeyState('1') == KeyboardStates::clicked) visibility_segment_number.append("1");
					if (key.GetKeyState('2') == KeyboardStates::clicked) visibility_segment_number.append("2");
					if (key.GetKeyState('3') == KeyboardStates::clicked) visibility_segment_number.append("3");
					if (key.GetKeyState('4') == KeyboardStates::clicked) visibility_segment_number.append("4");
					if (key.GetKeyState('5') == KeyboardStates::clicked) visibility_segment_number.append("5");
					if (key.GetKeyState('6') == KeyboardStates::clicked) visibility_segment_number.append("6");
					if (key.GetKeyState('7') == KeyboardStates::clicked) visibility_segment_number.append("7");
					if (key.GetKeyState('8') == KeyboardStates::clicked) visibility_segment_number.append("8");
					if (key.GetKeyState('9') == KeyboardStates::clicked) visibility_segment_number.append("9");
				}


				//---Iterate through primitive segments in level 0 surface segments 
				if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('N') == KeyboardStates::clicked)
				{					
					SegmentsGraph::SurfaceSegment * crtSegment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(currentlyViewedSegment);
					if (crtSegment != NULL) 
					{
						//If an internal graph exists for this segment ... set visible the next segment in the graph.
						if (crtSegment->internalSegGraph != NULL) 
						{
							crtSegment->internalGraphSelectionIndex += 1;
							if (crtSegment->internalGraphSelectionIndex > crtSegment->internalSegGraph->SurfaceSegmentsCount()-1) crtSegment->internalGraphSelectionIndex = 0;
							std::cout << "[Main] Showing Next Primitive id=" << crtSegment->internalGraphSelectionIndex << " in Segment with id=" << currentlyViewedSegment << std::endl;
							((KDTreePointCloud*)(gp->GetShape()))->SetVisibleNextPrimitiveInSegment(currentlyViewedSegment, crtSegment->internalGraphSelectionIndex);
						}
						else
						{
							std::cout << "[Main] Segment " << currentlyViewedSegment << " does not have an internal graph " << std::endl;
						}
					}
					accuReset = true;
				}

				//---Write The structure graph to file.
				if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('G') == KeyboardStates::clicked)
				{
					std::cout << "[Main] Drawing structure graph to file" << std::endl;
					pcs->m_sceneToSolve->DrawSegmentsGraph(pcs->m_sceneToSolve->m_segmentsGraph, pcs->m_sceneToSolve->m_pointCloud->GetName());
					pcs->m_sceneToSolve->DrawSceneGraph(pcs->m_sceneToSolve->m_structureGraph, pcs->m_sceneToSolve->m_pointCloud->GetName(), 1.2f);
				}

				//---Show points which have not been fitted by RANSAC
				if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('M') == KeyboardStates::clicked)
				{										
					SegmentsGraph::SurfaceSegment * crtSegment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(currentlyViewedSegment);
					//If an internal graph exists for this segment ... set visible the next segment in the graph.
					if (crtSegment->internalSegGraph != NULL) 
					{							
						std::cout << "[Main] Showing Non Fitted Points in Segment with id " << currentlyViewedSegment << std::endl;
						((KDTreePointCloud*)(gp->GetShape()))->SetVisibleNonFittedPointsInSegment(currentlyViewedSegment);
					}
					else
					{
						std::cout << "[Main] Segment " << currentlyViewedSegment << " does not have an internal graph " << std::endl;
					}
					accuReset = true;
				}

				//---Switch pointclouds 
				if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('P') == KeyboardStates::clicked)
				{							
					processing_pointcloud_switch_request = true;
					std::cout << "[Main] Input Pointcloud ID ... " << std::endl;
					new_pointcloud_id.clear();
					new_pointcloud_view.clear();
				}

				if (processing_pointcloud_switch_request && key.GetKeyState('P') == KeyboardStates::clicked)
				{	
					processing_pointcloud_switch_request = false;
					processing_pointcloud_switch_request_view = false;
					accuReset = true;
					try
					{
						processing_visibility_segmentid = false;
						int pointcloud_id = boost::lexical_cast<int, std::string>(new_pointcloud_id);
						std::cout << "[Main] Loading Pointcloud with id " << new_pointcloud_id << std::endl;
						if (pointcloud_id != 0)
						{
							if (pcs->m_pPreloadedScenes->find(pointcloud_id) == pcs->m_pPreloadedScenes->end() || new_pointcloud_view.length() > 0)
							{
								bool loaded = false;
								int pointcloud_view = 0;
								if (new_pointcloud_view.length() > 0)
								{
									pointcloud_view = boost::lexical_cast<int, std::string>(new_pointcloud_view);
									pointcloud_id += pointcloud_view * 100;
								}
								PointCloudSegmentation * pcs_loaded = new PointCloudSegmentation(pcs->m_pEnvironment, "Output/PCS/"+new_pointcloud_id+"/", pointcloud_id, loaded);
								if (!loaded)
								{
									//Loading Failed
									std::cout << "[Main] Pointcloud with id " << new_pointcloud_id << " FAILED." << std::endl;
								}
								else
								{
									std::cout << "[Main] Pointcloud with id " << new_pointcloud_id << " LOADED." << std::endl;
									pcs->m_sceneToSolve = pcs_loaded;							
									gp->SetShape(pcs_loaded->m_pointCloud);
									pcs_loaded->m_pointCloud->SetEnvironment(pcs->m_pEnvironment);
									(*(pcs->m_pPreloadedScenes))[pointcloud_id] = pcs_loaded;
									((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->ClearAllGrids();
									((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->ClearAllOBBs();
									std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());
									if (camera_parameters->find(new_pointcloud_id) != camera_parameters->end())
									{
										std::cout << "[Setting Camera Parameters] - Trunk " << new_pointcloud_id << " found. Resetting Camera Position" << std::endl;
										std::pair<std::vector<Vector3>, std::vector<float>> cam_parameters = camera_parameters->find(new_pointcloud_id)->second;
										pCamera->MoveTo(cam_parameters.first[0]);
										pCamera->Look(cam_parameters.first[1], cam_parameters.first[2]);
										theta_x = cam_parameters.second[0];
										theta_y = cam_parameters.second[1];
										theta_z = cam_parameters.second[2];
										gp->WorldTransform.SetRotation(Matrix3x3::CreateRotation(Vector3::UnitXPos, theta_x) * 
																		Matrix3x3::CreateRotation(Vector3::UnitYPos, theta_y) * 
																		Matrix3x3::CreateRotation(Vector3::UnitZPos, theta_z));
										accuReset=true;
									}
								}
							}
							else
							{
								std::cout << "[Main] Loading Pointcloud #" << new_pointcloud_id << " from cache." << std::endl;
								gp->SetShape(pcs->m_pPreloadedScenes->at(pointcloud_id)->m_pointCloud);
							}
						}
						else
						{
							std::cout << "[Main] Resetting Scene to Original." << std::endl;
							gp->SetShape(pcs->m_sceneToSolve->m_pointCloud);
						}							
					} catch (boost::bad_lexical_cast) {				
						std::cout << "[EXCEPTION THROWN] Boost::Bad_lexical_cast" << std::endl;
					}
				}

				if (processing_pointcloud_switch_request && key.GetKeyState('V') == KeyboardStates::clicked)
				{										
					std::cout << "[Main] Input Pointcloud ID View number for... " << new_pointcloud_id << std::endl;			
					new_pointcloud_view.clear();
					processing_pointcloud_switch_request_view = true;
				}

				if (processing_pointcloud_switch_request)
				{
					if (!processing_pointcloud_switch_request_view)
					{
						if (key.GetKeyState('0') == KeyboardStates::clicked) new_pointcloud_id.append("0");
						if (key.GetKeyState('1') == KeyboardStates::clicked) new_pointcloud_id.append("1");
						if (key.GetKeyState('2') == KeyboardStates::clicked) new_pointcloud_id.append("2");
						if (key.GetKeyState('3') == KeyboardStates::clicked) new_pointcloud_id.append("3");
						if (key.GetKeyState('4') == KeyboardStates::clicked) new_pointcloud_id.append("4");
						if (key.GetKeyState('5') == KeyboardStates::clicked) new_pointcloud_id.append("5");
						if (key.GetKeyState('6') == KeyboardStates::clicked) new_pointcloud_id.append("6");
						if (key.GetKeyState('7') == KeyboardStates::clicked) new_pointcloud_id.append("7");
						if (key.GetKeyState('8') == KeyboardStates::clicked) new_pointcloud_id.append("8");
						if (key.GetKeyState('9') == KeyboardStates::clicked) new_pointcloud_id.append("9");
					}
					else
					{
						if (key.GetKeyState('0') == KeyboardStates::clicked) new_pointcloud_view.append("0");
						if (key.GetKeyState('1') == KeyboardStates::clicked) new_pointcloud_view.append("1");
						if (key.GetKeyState('2') == KeyboardStates::clicked) new_pointcloud_view.append("2");
						if (key.GetKeyState('3') == KeyboardStates::clicked) new_pointcloud_view.append("3");
						if (key.GetKeyState('4') == KeyboardStates::clicked) new_pointcloud_view.append("4");
						if (key.GetKeyState('5') == KeyboardStates::clicked) new_pointcloud_view.append("5");
						if (key.GetKeyState('6') == KeyboardStates::clicked) new_pointcloud_view.append("6");
						if (key.GetKeyState('7') == KeyboardStates::clicked) new_pointcloud_view.append("7");
						if (key.GetKeyState('8') == KeyboardStates::clicked) new_pointcloud_view.append("8");
						if (key.GetKeyState('9') == KeyboardStates::clicked) new_pointcloud_view.append("9");
					}
				}

				return accuReset;
			}			
		
		};
	}
}